package trb.complexshield.voxel;

import java.awt.Color;
import java.util.Arrays;
import javax.vecmath.Point3f;
import no.hrp.hvrc.rad.calc.intersect.IntersectableBox;
import no.hrp.hvrc.rad.calc.intersect.Segment;
import no.hrp.hvrc.utils.j3d.Mat4;
import no.hrp.hvrc.utils.j3d.Vec3;
import no.hrp.hvrc.utils.swing.UIUtils;
import trb.complexshield.ComplexShield;
import trb.complexshield.ComplexShieldOutput;

public class VoxelGridBox implements ComplexShield {

    public static void main(String[] args) {
        IntersectableBox intersectableBox = new IntersectableBox(0, new Vec3(-1, -1, -1), new Vec3(1, 1, 1), new Mat4().translate(1, 0, 0));
        VoxelGrid voxelGrid = new VoxelGrid(16, 16, 16);
        voxelGrid.bitSet.set(0, voxelGrid.bitSet.size());
        VoxelGridBox voxelGridBox = new VoxelGridBox(voxelGrid, intersectableBox);
        ComplexShieldOutput output = new ComplexShieldOutput();
        voxelGridBox.intersect(new Segment(new Vec3(-5, 0, 0), new Vec3(5, 0, 0)), output);
        System.out.println(output.data);
    }

    public static void debugTest1() {
        IntersectableBox intersectableBox = new IntersectableBox(0, new Vec3(-1, -1, -1), new Vec3(1, 1, 1), new Mat4().translate(1, 0, 0));
        VoxelGridBox voxelGridBox = new VoxelGridBox(null, intersectableBox);
        Vec3 startPos = new Vec3();
        Vec3 endPos = new Vec3();
        float[] tMinMaxOut = new float[2];
        voxelGridBox.getNormalisedIntersections(new Segment(new Vec3(-5, 0, 0), new Vec3(5, 0, 0)), startPos, endPos, tMinMaxOut);
        System.out.println(startPos + " should be (0.0, 0.5, 0.5)");
        System.out.println(endPos + " should be (1.0, 0.5, 0.5)");
        System.out.println(Arrays.toString(tMinMaxOut) + " should be 0.5, 0.7");

        VoxelGrid voxelGrid = new VoxelGrid(16, 16, 16);
        startPos.scale(voxelGrid.xdim, voxelGrid.ydim, voxelGrid.zdim);
        endPos.scale(voxelGrid.xdim, voxelGrid.ydim, voxelGrid.zdim);
        Line line = new Line(startPos, endPos);

        System.out.println("line " + startPos + " " + endPos);

        RaytraceApplication.populateCylinder(voxelGrid);
        Raytrace raytrace = new Raytrace(voxelGrid);
        Visualisation visualisation = new Visualisation(voxelGrid, 32);
        visualisation.clear(Color.WHITE);
        visualisation.paintVoxelGrid(Color.LIGHT_GRAY);
        visualisation.drawGrid(Color.BLACK);
        visualisation.drawLine(line, Color.GREEN);
        raytrace.raytrace(line, visualisation);
        UIUtils.showImageInJFrame(visualisation.image);
    }

    public final VoxelGrid voxelGrid;
    public final Raytrace raytrace;
    public final IntersectableBox intersectableBox;

    public VoxelGridBox(VoxelGrid voxelGrid, IntersectableBox intersectableBox) {
        this.voxelGrid = voxelGrid;
        this.intersectableBox = intersectableBox;
        raytrace = new Raytrace(voxelGrid);
    }

    @Override
    public void intersect(Segment line, final ComplexShield.Output output) {
        if (line.length() <= 0) {
            return;
        }
        Vec3 inBoxStart = new Vec3();
        Vec3 inBoxEnd = new Vec3();
        final float[] tMinMax = new float[2];
        if (getNormalisedIntersections(line, inBoxStart, inBoxEnd, tMinMax)) {
            inBoxStart.scale(voxelGrid.xdim, voxelGrid.ydim, voxelGrid.zdim);
            inBoxEnd.scale(voxelGrid.xdim, voxelGrid.ydim, voxelGrid.zdim);
            Line raytraceLine = new Line(inBoxStart, inBoxEnd);
            raytrace.raytrace(raytraceLine, new Raytrace.Output() {
                @Override
                public void addIntersection(Line line, int x, int y, int z, float t) {
//                    System.out.println("addIntersection " + x + " " + y + " " + z + " t=" + t);

                }

                @Override
                public void addSegment(Line line, float t1, float t2) {
                    float out1 = tMinMax[0] + t1 * (tMinMax[1] - tMinMax[0]);
                    float out2 = tMinMax[0] + t2 * (tMinMax[1] - tMinMax[0]);
                    output.addIntersection(out1, out2);
                }
            });
        }
    }

    public boolean getNormalisedIntersections(Segment line, Vec3 start, Vec3 end, float[] tMinMaxOut) {
        Point3f origin = line.originRef();
        Point3f target = line.targetRef();
        float[] w2l = intersectableBox.getWorldToLocal().toFloats();
        float[] minMax = intersectableBox.getMinMax();
        boolean found = getLengthInside(origin.x, origin.y, origin.z, target.x, target.y, target.z, minMax, 0, w2l, 0, start, end, tMinMaxOut);
        start.x = (start.x - minMax[0]) / (minMax[3] - minMax[0]);
        start.y = (start.y - minMax[1]) / (minMax[4] - minMax[1]);
        start.z = (start.z - minMax[2]) / (minMax[5] - minMax[2]);
        end.x = (end.x - minMax[0]) / (minMax[3] - minMax[0]);
        end.y = (end.y - minMax[1]) / (minMax[4] - minMax[1]);
        end.z = (end.z - minMax[2]) / (minMax[5] - minMax[2]);
        return found;
    }

    public static boolean getLengthInside(float originx, float originy, float originz
            , float targetx, float targety, float targetz
            , float[] minMax, int minMaxOff, float[] w2l, int w2lOff, Vec3 startOut, Vec3 endOut, float[] tMinMaxOut) {
        // transform start point into box local space
        float x1 = w2l[w2lOff + 0] * originx + w2l[w2lOff + 1] * originy + w2l[w2lOff + 2]  * originz + w2l[w2lOff + 3];
        float y1 = w2l[w2lOff + 4] * originx + w2l[w2lOff + 5] * originy + w2l[w2lOff + 6]  * originz + w2l[w2lOff + 7];
        float z1 = w2l[w2lOff + 8] * originx + w2l[w2lOff + 9] * originy + w2l[w2lOff + 10] * originz + w2l[w2lOff + 11];
        float x2 = w2l[w2lOff + 0] * targetx + w2l[w2lOff + 1] * targety + w2l[w2lOff + 2]  * targetz + w2l[w2lOff + 3];
        float y2 = w2l[w2lOff + 4] * targetx + w2l[w2lOff + 5] * targety + w2l[w2lOff + 6]  * targetz + w2l[w2lOff + 7];
        float z2 = w2l[w2lOff + 8] * targetx + w2l[w2lOff + 9] * targety + w2l[w2lOff + 10] * targetz + w2l[w2lOff + 11];

        // create normalised direction vector
        float directionX = x2 - x1;
        float directionY = y2 - y1;
        float directionZ = z2 - z1;
        float directionLength = (float) Math.sqrt(directionX * directionX + directionY * directionY + directionZ * directionZ);
        float ooDirectionLength = 1.0f / directionLength;
        directionX *= ooDirectionLength;
        directionY *= ooDirectionLength;
        directionZ *= ooDirectionLength;

        float invDirectionX = 1.0f / directionX;
        float invDirectionY = 1.0f / directionY;
        float invDirectionZ = 1.0f / directionZ;

        int signX = (invDirectionX < 0 ? 1 : 0);
        int signY = (invDirectionY < 0 ? 1 : 0);
        int signZ = (invDirectionZ < 0 ? 1 : 0);

        // algorithm taken from: http://www.cs.utah.edu/~awilliam/box/
        // is accurate and optimized
        float tmin  = (minMax[minMaxOff +      signX  * 3 + 0] - x1) * invDirectionX;
        float tmax  = (minMax[minMaxOff + (1 - signX) * 3 + 0] - x1) * invDirectionX;
        float tymin = (minMax[minMaxOff +      signY  * 3 + 1] - y1) * invDirectionY;
        float tymax = (minMax[minMaxOff + (1 - signY) * 3 + 1] - y1) * invDirectionY;
        if ((tmin > tymax) || (tymin > tmax)) {
            return false;
        }
        if (tmin < tymin) {
            tmin = tymin;
        }
        if (tmax > tymax) {
            tmax = tymax;
        }
        float tzmin = (minMax[minMaxOff +      signZ  * 3 + 2] - z1) * invDirectionZ;
        float tzmax = (minMax[minMaxOff + (1 - signZ) * 3 + 2] - z1) * invDirectionZ;
        if ((tmin > tzmax) || (tzmin > tmax)) {
            return false;
        }
        if (tmin < tzmin) {
            tmin = tzmin;
        }
        if (tmax > tzmax) {
            tmax = tzmax;
        }
        if (tmin < 0) {
            tmin = 0;
        }
        if (tmax > directionLength) {
            tmax = directionLength;
        }

        startOut.set(x1 + tmin * directionX, y1 + tmin * directionY, z1 + tmin * directionZ);
        endOut.set(x1 + tmax * directionX, y1 + tmax * directionY, z1 + tmax * directionZ);
        tMinMaxOut[0] = tmin * ooDirectionLength;
        tMinMaxOut[1] = tmax * ooDirectionLength;
        return true;
    }

    public Vec3 getWorld(float gridx, float gridy, float gridz) {
        Mat4 localToWorld = new Mat4(intersectableBox.getLocalToWorld());
        float[] minMax = intersectableBox.getMinMax();
        localToWorld.translate(minMax[0], minMax[1], minMax[2]);
        localToWorld.scale(minMax[3]-minMax[0], minMax[4]-minMax[1], minMax[5]-minMax[2]);
        return localToWorld.transformAsPoint(new Vec3(gridx/voxelGrid.xdim, gridy/voxelGrid.ydim, gridz/voxelGrid.zdim));
    }
}
